/*
 * File - ros_webcam_publisher.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */

#include "config.h"
#include <nodelet/nodelet.h>
#include <ros/rate.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/distortion_models.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

/**
 * \endcond
 */

#include "ros_webcam_publisher.h"
#include "inudev_ros_nodelet.h"

/**
 * \file ros_webcam_publisher.cpp
 *
 * \brief CRosWebcamPublisher
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id: 2466921dbf29853129684b9dca43864308a13c1e $";

/**
 * \endcond
 */

using namespace InuDev;

namespace __INUROS__NAMESPACE__
{
    CRosWebcamPublisher::CRosWebcamPublisher(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, CRosSensor* _rosSensor, std::string iTopicName)
        : CRosPublisher(_node, _image_transport, "webcam", _rosSensor)
        , publisher(imageTransport->advertiseCamera(iTopicName, 1))
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName()); 
    }

    void CRosWebcamPublisher::fillCameraInfoFromWebcam(unsigned int frameIndex, int width, int height)
    {

        ros::param::param<std::string>("Device_ID", mWebCamFrameID, "");
        mCi.header.frame_id  = "camera_link"+mWebCamFrameID;
        mCi.header.seq       = frameIndex;
        mCi.width            = width;
        mCi.height           = height;
        mCi.distortion_model = "plumb_bob";

        if (!mSensors.empty())
        {
            auto c = mCalibrationData->Sensors[mSensors.at(0)].RealCamera.Intrinsic;

            // Intrinsic camera matrix for the raw (distorted) images.
            mCi.K[0] = mCi.P[0]  = c.FocalLength[0];
            mCi.K[1] = mCi.P[1]  = 0.0f;
            mCi.K[2] = mCi.P[2]  = c.OpticalCenter[0];
            mCi.K[3] = mCi.P[3]  = 0.0f;
            mCi.K[4]             = c.FocalLength[1];
                       mCi.P[4]  = 0;
                       mCi.P[5]  = c.FocalLength[1];
            mCi.K[5] = mCi.P[6]  = c.OpticalCenter[1];
            mCi.K[6] = mCi.P[7]  = 0.0f;
            mCi.K[7] = mCi.P[8]  = 0.0f;
            mCi.K[8]             = 1.0f;
                       mCi.P[9]  = 0.0f;
                       mCi.P[10] = 1.0f;
                       mCi.P[11] = 0.0f;

            mCi.R[0] = mCi.R[4] = mCi.R[8] = 1.0f;
            mCi.R[1] = mCi.R[2] = mCi.R[3] = mCi.R[5] = mCi.R[6] = mCi.R[7] = 0.0f;

            mCi.D.resize(c.LensDistortion.size());

            for (std::vector<double>::size_type i = 0; i < mCi.D.size(); i++)
            {
                mCi.D.at(i) = c.LensDistortion.at(i);
            }
        }
    }

    void CRosWebcamPublisher::FrameCallback(std::shared_ptr<InuDev::CImageStream> iStream, std::shared_ptr<const InuDev::CImageFrame> iFrame, InuDev::CInuError iError)
    {
        NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (!CheckFrame(iFrame, iError))
        {
            return;
        }

        InuDev::EImageFormat format = (InuDev::EImageFormat)iFrame->Format();
            
        int width = iFrame->Width();
        int height = iFrame->Height();
        int bpp = iFrame->BytesPerPixel();
        const InuDev::byte *data = iFrame->GetData();

        // cv::Mat frame(height, width, CV_8UC3, (char *)data); // Hack due to openCV runtime error on cv::mat frame
        cv::Mat frame;//(height, width, CV_8UC3, (char *)data); // Hack due to openCV runtime error on cv::mat frame



        // if (format == InuDev::EImageFormat::eBGR)
        // {
        //     cv::Mat registeredFrame(height, width, CV_8UC3, (char *) data);
        //     frame = registeredFrame;
        // }
        // else
        // {
        //      cv::Mat defaultFrame(height, width, CV_8UC4, (char *)data);
        //      frame = defaultFrame;
        // }
        switch (format)
        {
            case InuDev::EImageFormat::eBGR:
                frame = cv::Mat(height, width, CV_8UC3, (char *)data);
                cv::cvtColor(frame, frame, CV_BGR2RGB);
                break;
            case InuDev::EImageFormat::eBGRA:
                frame = cv::Mat(height, width, CV_8UC4, (char *)data);
                cv::cvtColor(frame, frame, CV_BGRA2RGB);
                break;
            case InuDev::EImageFormat::eRGBA:
                frame = cv::Mat(height, width, CV_8UC4, (char *)data);
                cv::cvtColor(frame, frame, CV_RGBA2RGB);
                break;
            default:
                NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": unrecognised WebCam frame foramt: " << format );
                return;

        }
        
        //save the latest image
        sensor->InsertWebImage(frame, iFrame->Timestamp);


        std_msgs::Header frameHeader;
        frameHeader.seq = iFrame->FrameIndex;
        frameHeader.stamp = mCi.header.stamp = ros::Time( iFrame->Timestamp / 1000000000ll, iFrame->Timestamp % 1000000000ll);

        ros::param::param<std::string>("Device_ID", mWebCamFrameID, "");
        frameHeader.frame_id = "camera_link"+mWebCamFrameID;
        //bgra8: CV_8UC4, BGR color image with an alpha channel
        sensor_msgs::ImagePtr msg; 

        // if (format == InuDev::EImageFormat::eBGR)
        // {
            msg = cv_bridge::CvImage(frameHeader, "rgb8", frame).toImageMsg();
        // }
        // else
        // {
        //     msg = cv_bridge::CvImage(frameHeader, "rgba8", frame).toImageMsg();
        // }

        fillCameraInfoFromWebcam(iFrame->FrameIndex, width, height);
        publisher.publish(*msg, mCi, ros::Time( iFrame->Timestamp / 1000000000ll, iFrame->Timestamp % 1000000000ll));

        if (CInuDevRosNodelet::config.webcam_verbose)
        {
            NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": WebCam frame: width=" << width << ", height=" << height << ", format=" << format << ", bpp=" << bpp);

            cv::imshow(CV_WINDOW_NAME, frame); // Show our image inside it.
            cv::waitKey(1);
        }
    }

    InuDev::CInuError CRosWebcamPublisher::RegisterCallback()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CImageStream::CallbackFunction callback = std::bind(&CRosWebcamPublisher::FrameCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

        return std::static_pointer_cast<InuDev::CImageStream>(stream)->Register(callback);
    }

    InuDev::CInuError CRosWebcamPublisher::UnregisterCallback()
    {
        return std::static_pointer_cast<InuDev::CImageStream>(stream)->Register(nullptr);
    }

    int CRosWebcamPublisher::GetNumSubscribers()
    {
        publisherPrev = publisherCurr;
        publisherCurr = publisher.getNumSubscribers();

        return publisherCurr;
    }

    InuDev::CInuError CRosWebcamPublisher::InitStream()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CInuError err(0);

        if (mPostProcessing != 0)
        {
            if(mPostProcessing | CImageStream::EPostProcessing::eRegistered && mRegistrationChannel != -1)
            {
                err = std::static_pointer_cast<InuDev::CImageStreamExt>(stream)->Init((CImageStream::EOutputFormat)mOutputFormat,(CImageStreamExt::EPostProcessing)mPostProcessing, mRegistrationChannel);
                NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << "Starting RGB with post processing : " << mPostProcessing << " With registration channel: " << mRegistrationChannel << "With output format: " << mOutputFormat);
            }
            else
            {
                err = std::static_pointer_cast<InuDev::CImageStreamExt>(stream)->Init((CImageStream::EOutputFormat)mOutputFormat,(CImageStreamExt::EPostProcessing)mPostProcessing, InuDev::DEFAULT_CHANNEL_ID);
                NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << "Starting RGB with post processing: " << mPostProcessing << "With output format: " << mOutputFormat);
            }
        }
        else
        {
            err = std::static_pointer_cast<InuDev::CImageStream>(stream)->Init((CImageStream::EOutputFormat)mOutputFormat);
            NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " Starting RGB with output format: " << mOutputFormat);
        }
      
    if (err != InuDev::EErrorCode::eOK)
    {
        NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
        return err;
    }
        return InuDev::EErrorCode::eOK;
}
    

    InuDev::CInuError CRosWebcamPublisher::StartStream()
    {
        // Load default params from server
        ros::param::param<int>("RGBChannel", mParamsChannelID, -1);
        ros::param::param<int>("RGBOutputFormat", mOutputFormat, -1);
        ros::param::param<int>("RGBPostProcessing", mPostProcessing, -1);
        ros::param::param<int>("RGBRegistarionChannel", mRegistrationChannel, -1);

        // check for user dynamic changes
        if(CInuDevRosNodelet::config.RGB_channel_ID != -1)
        {
            mParamsChannelID = CInuDevRosNodelet::config.RGB_channel_ID;
        }

        if(CInuDevRosNodelet::config.RGB_output_format != 0)
        {
            mOutputFormat = CInuDevRosNodelet::config.RGB_output_format;
        }

        if(CInuDevRosNodelet::config.RGB_post_processing != 0)
        {
            mPostProcessing = CInuDevRosNodelet::config.RGB_post_processing;
        }

        if(CInuDevRosNodelet::config.RGB_registration_channel != -1)
        {
            mRegistrationChannel = CInuDevRosNodelet::config.RGB_registration_channel;
        }

        if (mParamsChannelID == -1)
        {
            stream = std::static_pointer_cast<InuDev::CBaseStream>(sensor->getSensor()->CreateImageStreamExt());
        }
        else
        {
            stream = std::static_pointer_cast<InuDev::CBaseStream>(sensor->getSensor()->CreateImageStreamExt(mParamsChannelID));
        }

        NODELET_ERROR_STREAM_COND(!stream, __INUROS_FUNCTION_NAME__ << ": failed creating " << getName());

        InuDev::EErrorCode err = InitStream();
       
        if (err != InuDev::EErrorCode::eOK)
        {   
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        err = CRosPublisher::StartStream();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        uint32_t channelID;
        err = stream->GetChannel(channelID);

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        mChannelID = int(channelID);

        mSensors = sensor->GetSensors(mChannelID);
        mCalibrationData = sensor->GetCalibrationData(mCurrentTemprature, mChannelID);
        for(int i = 0; i < 5; i ++)
        {
            if (i == 2)
            {
    NODELET_WARN_STREAM( "Web Sen " << i <<  ",FocalLength:" << mCalibrationData->Sensors[i].RealCamera.Intrinsic.FocalLength[0] << "," << mCalibrationData->Sensors[i].RealCamera.Intrinsic.FocalLength[1] );
    NODELET_WARN_STREAM("OpticalCenter:" << mCalibrationData->Sensors[i].RealCamera.Intrinsic.OpticalCenter[0] << "," << mCalibrationData->Sensors[i].RealCamera.Intrinsic.OpticalCenter[1] );
            }
            else
            {
    NODELET_WARN_STREAM( "Web Sen " << i <<  ",FocalLength:" << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.FocalLength[0] << "," << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.FocalLength[1] );
    NODELET_WARN_STREAM("OpticalCenter:" << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.OpticalCenter[0] << "," << mCalibrationData->Sensors[i].VirtualCamera.Intrinsic.OpticalCenter[1] );
            }
        }

        return InuDev::EErrorCode::eOK;
    }
}
